/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief This is an example application for using the J2B2ClientInterface.
 * \author Antti Maula <antti.maula@tkk.fi>
 *
 * This file is intended to be just an example. You can freely rewrite
 * this (and even the J2B2ClientInterface part if you like) The
 * J2B2ClientInterface is just a initialization wrapper over the MaCI
 * Client interfaces. There is also a 'Demonstration' for all
 * interfaces in file 'J2B2Demo.cpp', which is executed from this
 * module after succesfull initialization.
 */
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string>
#include <vector>
#include "owndebug.h"
#include "ownutils.h"
#include "binbag.h"
#include "J2B2ClientInterface.hpp"
#include "J2B2Demo.hpp"

//*****************************************************************************
// Include GIMI in proper way
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************

// Set initial verbosity level to 1
static int verbose = 1;
//*****************************************************************************

// Choose your namespace
using namespace MaCI;
//*****************************************************************************

// Running?
static bool isSimulator = true; ///< Is the backend Simulator (not all interfaces available)
static volatile bool run = true;
static ownThreadHandle signalHandlerThread;  
static gimi::GIMI g;
//*****************************************************************************

// Application specific configuration (could be) encapsulated here.
struct SJ2B2ClientConfig
{
  SJ2B2ClientConfig()
    : username() {}

  std::string username; ///< Example field.
};
//*****************************************************************************

// GIMnet parameters
std::string gimnetAP = "asrobo.hut.fi";
int gimnetAPPort = 52006;
std::string gimnetName;
std::string macigroup = "FSRSim_G6"; // Default MaciGroup for J2B2 controller is 'J2B2'
SJ2B2ClientConfig globalConfig;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

// Signal handler thread. This sits and waits for signals. This way
// other threads in the application won't be unexpectedly interrupted
// by signals.
static int SignalHandlerThread(void *)
{
  dPrint(8, "Signal handler thread (%ld) starting...", pthread_self());

  while (run) {
    // Now, wait for requested signals to arrive
    int sig;
    sigset_t mask;
    sigemptyset(&mask);
    sigaddset(&mask, SIGINT);
    sigaddset(&mask, SIGTERM);
    sigaddset(&mask, SIGPIPE);
    sigaddset(&mask, SIGHUP);
    sigwait(&mask, &sig);

    // Got signal, sigwait returned!
    dPrint(1,"Got Signal '%d' (%s)", sig, strsignal(sig));

    // What to do?
    switch(sig) {
    case SIGINT:
    case SIGTERM:
    case SIGPIPE:
      g.stop();
      run = false;
      ownSleep_ms(1000);
      break;
      
    case SIGHUP:
    default:
      dPrint(1,"Unhandled signal! Ignore!");
      break;
    }
  }
  return 0;
}
//*****************************************************************************

static void InitSignalHandler(void)
{
  // Now; Set to BLOCK ALL SIGNALS
  sigset_t mask;
  sigemptyset(&mask);
  sigaddset(&mask, SIGINT);
  sigaddset(&mask, SIGTERM);
  sigaddset(&mask, SIGPIPE);
  sigaddset(&mask, SIGHUP);
  pthread_sigmask(SIG_BLOCK, &mask, NULL);
  
  // Start signal handler thread
  signalHandlerThread = ownThread_Create((void*)SignalHandlerThread, NULL);
}
//*****************************************************************************
//*****************************************************************************

// Show basic usage.
static void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tImageClientExample [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tAttempt to Initialize J2B2 in MaCIGroup 'name'\n");
  
  fprintf(stderr,"  [-M mode]\t\t\tClient mode, select 'real' or 'sim'\n");

  
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************
//*****************************************************************************

void RunDemo(CJ2B2ClientInterface &ci)
{
  // \note ************* READ THIS ************
  // This function is the ENTRY POINT for your application. Replace
  // the contents of this function, and use the given
  // CJ2B2ClientInterface for any robot communication. At this point,
  // all initializations are succesfully made, so you can concentrate
  // on building your application.

  // Construct an instance of the demo application, giving the
  // CJ2B2ClientInterface as parameter. (Is stored inside the class
  // instance as reference)
  CJ2B2Demo d(ci);

  // Execute the demonstration. Will start a bunch of threads
  // executing different tasks. This function returns immediately
  // after the demonstration is started.
  d.Execute();

  // This will wait forever until the 'run' flag changes state. The
  // run flag is only modified by the signal handler function, so
  // giving a SIGINT or SIGTERM signal to the process will gracefully
  // terminate the application.
  while(run) {
    dPrint(8,"Still waiting...");
    ownSleep_ms(5000);
  }

  // Terminate the demostration. This will attempt to gracefully close
  // and terminate all the demonstration threads before returning
  // executing to this module.
  d.Terminate();
}
//*****************************************************************************

// Execute client. This function is called from the main() routine
// after succesfully connected to GIMnet.
static int ExecuteClient()
{
  using namespace MaCI;
  int result = 0;
  bool r = false;
  CJ2B2ClientInterface ci(&g);
  
  // Select mode, depending on value of 'isSimulator'
  if (isSimulator) {
    r = ci.Open(macigroup, KEnabledInterfacesForSimulator);

  } else {
    // Default; no ExtraCam
    r = ci.Open(macigroup, KEnabledInterfacesForRealJ2B2);

    /// FOR EXTRACAM SUPPORT: Uncomment this (And comment out the one
    /// above) instead if you wish to use the Extra camera too.
    // r = ci.Open(macigroup, KEnabledInterfacesForRealJ2B2|KEnabledInterfaceImageCameraExtra);
  }
  
  // Check for Open() status.
  if (r) {
    dPrint(1,"Succesfully Initialized J2B2ClientInterface using MaCIGroup '%s'!", 
           macigroup.c_str());
    
    // Do your work here.
    RunDemo(ci);

  } else {
    dPrint(1,"Failed to initialize J2B2ClientInterface using Macigroup '%s'! Fatal error.",
           macigroup.c_str());
    result = -1;

  }

  return result;
}
//*****************************************************************************

static bool ParseConfiguration(SJ2B2ClientConfig &aConf,
                               const std::string &aConfigFile)
{ 
  gim::CSettings settings;
  bool result = settings.ReadSettingsXMLFile(aConfigFile);
  
  // Was the file succesfully parsed?
  if (result) {
    bool r;
    
    // Set basekey.
    settings.SetBaseKey("J2B2Client");

    // This is the place where you can read any parameters you
    // wish. Just add them to the SJ2B2ClientConfig structure, and
    // read them in here.

    // Start extracting parameters. Duplicate this to extract
    // more. The print is just for verbalizing the output, so use it
    // if you want.
    r = settings.GetValue("username", aConf.username);
    if (r) { dPrint(3,"Got configuration value for 'username' = '%s'", aConf.username.c_str()); } 
    else { dPrint(3,"No configuration value for 'username'"); }
    
    // Modify 'result', for example, if you want to force some parameters to
    // exists or pass some other quality check.
  }
  
  return result;
}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  // setup signal handling
  InitSignalHandler();

  // Conf
  std::string configurationFile;

  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(2);
  const std::string logfile = std::string(argv[0]) + ".log";
  debugSetLogFilename(logfile.c_str());

  // Greet
  fprintf(stderr,
	  "\n%s - J2B2 Example control application\n\n", argv[0]);

  ///////////////////////////////////////////////////////////////////////////// 
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "M:f:G:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////
    case 'M':
      if (optarg == std::string("sim")) {
        isSimulator = true;
      } else if (optarg == std::string("real")) {
        isSimulator = false;
      }
      break;
      
    case 'f':
      configurationFile = optarg;
      dPrint(3,"Using configuration file: '%s'", optarg);
      if (ParseConfiguration(globalConfig, configurationFile) == true) {
        dPrint(4,"Succesfully parsed configuration file '%s'", optarg);
      } else {
        dPrint(1,"Parsing configuration failed! Fatal error. Exit.");
        exit(1);
      }
      break;
      
    case 'G':
      macigroup = optarg;
      dPrint(3,"Using MaCIGroup '%s'", optarg);
      break;
      
      /////////////////////////////////////////////////////////////////////////
      /// Add here anything you need.
      /////////////////////////////////////////////////////////////////////////

      /////////////////////////////////
      ///// MaCI/GIMnet parameters ////
      /////////////////////////////////
    case 'n':
      gimnetName = optarg;
      gimnetName.erase(31);
      dPrint(3,"Using GIMnet Name '%s'", gimnetName.c_str());
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  
  if (isSimulator) {
    dPrintLCGreen(1,"Note; Running in Simulation mode!");

  } else {
    dPrintLCRed(1,"Note; Running in Real mode! (This means, you will be connected to real Robot!");

  }

  /////////////////////////////////////////////////////////////////////////////
  // Execute connection to GIMnet.
  /////////////////////////////////////////////////////////////////////////////
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
    // GIMnet connection OK, now execute module.
    r = ExecuteClient();
  }

  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
